import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.objectdetection.RangeFeatureDetector;

public class Controller extends Thread {

	/**
	 * Objects
	 */
	ThreadCommunication comm;
	DifferentialPilot pilot;
	UltrasonicSensor us;
	RangeFeatureDetector fd;
	boolean turnHeadRight = true; // direction for scanning

	/**
	 * Allocates pilot objects for driving & head movement, allocates an
	 * ultrasonic unit with a fixed scan range of one meter.
	 * 
	 * @param comm
	 *            - communication link with others threads.
	 */
	public Controller(ThreadCommunication comm) {
		this.comm = comm;
		// Used to be 11.25
		pilot = new DifferentialPilot(5.6f, 12.9f, Motor.A, Motor.B);
		pilot.setRotateSpeed(75f);
		pilot.setTravelSpeed(10.0f);
		us = new UltrasonicSensor(SensorPort.S1); // adjust according to what
		fd = new RangeFeatureDetector(us, 100, 500);
	}

	public void move() throws InterruptedException {
		ReceivedMessage msg;
		int scanVal[] = null;
		while (true) {
			msg = comm.getReceivedMessage();
			if (msg != null) {
				if (msg.getDrive() != -10 || msg.getTurn() != -10) {
					Sound.beepSequence();
					comm.addResponseMessage(new ResponseMessage(0, 0, 0, 0, 0,
							null, false));
					if (msg.getDrive() == 1)
						pilot.forward();
					else if (msg.getDrive() == -1)
						pilot.backward();
					else if (msg.getDrive() == 0)
						pilot.stop();

					if (msg.getTurn() == 1)
						pilot.rotateRight();
					else if (msg.getTurn() == -1)
						pilot.rotateLeft();
					else if (msg.getTurn() == 0)
						pilot.stop();
				} else {
					if (msg.getMove() != 0)
						pilot.travel(msg.getMove());
					if (msg.getRotate() != 0)
						pilot.rotate(msg.getRotate());
					if (msg.getNumScans() != 0) {
						scanVal = scan(msg.getNumScans());
					}
					comm.addResponseMessage(new ResponseMessage(msg.getMove(),
							msg.getMove(), msg.getRotate(), msg.getRotate(),
							msg.getNumScans(), scanVal, false));
				}
			}
		}
	}

	/**
	 * Uses the intitated ultrasonic unit and scans the degree directions
	 * 0,90,180,270. Turns it�s head every second time to it�s left for better
	 * calibration from motor C.
	 * 
	 * @param scanner
	 * @return returns the scanned distance
	 * @throws InterruptedException 
	 */
	public int[] scan(int scanner) throws InterruptedException {
		int scanVal[] = new int[scanner];
		int tempscans[] = new int[3];

		Motor.C.setSpeed(60);
		
		for (int i = 0; i < scanner; i++) {
			int deg = 360 / (scanner) + 1;
			tempscans[0] = us.getDistance();
			Thread.sleep(20);
			tempscans[1] = us.getDistance();
			Thread.sleep(20);
			tempscans[2] = us.getDistance();
			Thread.sleep(20);
			scanVal[i] = (tempscans[0] + tempscans[1] + tempscans[2]) / 3;
			if (turnHeadRight)
				Motor.C.rotate(deg);
			else
				Motor.C.rotate(-deg);
		}
		Motor.C.setSpeed(720);
		if (turnHeadRight)
			Motor.C.rotate(-360);
		else
			Motor.C.rotate(360);
		
		turnHeadRight = !turnHeadRight;

		if (turnHeadRight) {
			int scanVal2[] = new int[scanner];
			scanVal2[0] = scanVal[0];
			for (int i = 1; i < scanner; i++) {
				scanVal2[i] = scanVal[scanner - i];
			}
			return scanVal2;
		}
		return scanVal;
	}

	/**
	 * Starts thread for for move ()
	 */
	public void run() {
		try {
			move();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		System.out.println("Message:" + comm.getReceivedMessage());
	}

}
